﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace TGInspect
{
    [Flags]
    public enum HomeSensor
    {
        Z = 1,
        OT,
        ORG,
        ABS,
    }

    [Flags]
    public enum Moni{
	    POS=0,
	    MPOS,
	    PERR,
	    APOS,
	    LPOS,
	    IPOS,
	    TPOS,
	    //7,
	    FSPD = 8,
	    CSPD,
	    TSPD,
	    TRQ,
	    //12,
	    //13,
	    OMN1 = 14,
	    OMN2,
        MOT_SUBC_SET_REF = 0x80,
    }

    public enum Dir
    {
        CW = 0,
        CCW,
    }

    public enum AccFilter
    {
        NONE = 0,				
        EXPO,				
        OPT_ACCFIL_SCURVE,			
    }

    public enum Stop
    {
        Slow = 0,
        Sudden,
    }

    


    public class ServoMotor : IUpdate
    {
        const uint IndexNum = 100;
        const uint AlarmIndex = 99;

        protected short MtPort;
        protected short MtNum;
        protected uint DefaultIndex;

        public Dir HomingDir { get; private set; }
        HomeSensor HomeMode { get; set; }
        public long HomeRoughSpeed { get; set; }
        public long HomeDetailSpeed { get; set; }

        public uint CurIndex { get; private set; }
        public uint CmdIndex { get; private set; }
        public long CurPos { get; private set; }
        public long CmdPos { get; private set; }
        public long RealPos { get; private set; }
        public long CmdSpeed { get; private set; }
        public short AccValue { get; private set; }
        public short MoveAvrFilter { get; private set; }
        public Dir MoveDir { get; private set; }
        public long[] PosTable;
        public long[] SpeedTable;

        // bit
        public bool Moving { get; private set; }
        public bool InPos { get; private set; }
        public bool Alarm { get; private set; }
        public bool Warn { get; private set; }
        public bool CCWOn { get; private set; }
        public bool CWOn { get; private set; }
        public bool ORGOn { get; private set; }
        public bool LCmpOn { get; private set; }
        public bool SVOn { get; private set; }
        public bool PowOn { get; private set; }
        public bool Rdy { get; private set; }
        public bool IsHome { get; private set; }
        public bool IsRdy { get; private set; }
        public bool IsCommand { get; private set; }


             

        RtTimer Ready;
        Status st;


        public ServoMotor(short mtPort, short mtNum, short accVal, Dir homingDir, HomeSensor homeSensor
            , uint defaultIndex=1, long roughHomeSpeed = 100, long detailHomeSpeed = 50)
        {
            this.MtPort = mtPort;
            if (mtNum >= T1.MaxSlvNum) throw new Exception("mtNum >= MaxSlvNum");
            this.MtNum = mtNum;

            long[] PosTable = new long[IndexNum];
            long[] SpeedTable = new long[IndexNum];

            st = new Status();
            Ready = new RtTimer();

            AccValue = AccValue;

            HomingDir = homingDir;
            HomeMode = homeSensor;
            HomeRoughSpeed = roughHomeSpeed;
            HomeDetailSpeed = detailHomeSpeed;
            HomingDir = Dir.CCW;
            IsHome = false;
            
            CurIndex = CmdIndex = AlarmIndex;
            CurPos = CmdPos = 0;
            MoveDir = Dir.CW;
            this.DefaultIndex = defaultIndex;
            
        }

        public void Update()
        {
            T1.MotionUpdate(MtPort, MtNum);

            Moving = !T1.IsDEN(MtPort, MtNum);
            InPos = T1.IsPSET(MtPort, MtNum);
            Alarm = T1.IsALM(MtPort, MtNum);
            Warn = T1.IsWARNG(MtPort, MtNum);
            CCWOn = T1.IsN_OT(MtPort, MtNum);
            CWOn = T1.IsP_OT(MtPort, MtNum);
            ORGOn = T1.IsEXT1(MtPort, MtNum);
            LCmpOn = T1.IsL_CMP(MtPort, MtNum);
            SVOn = T1.IsSVON(MtPort, MtNum);
            RealPos = T1.GetRealCount(MtPort, MtNum);
            IsRdy = SVOn && !Alarm && IsHome && InPos && !Moving;

            if (Alarm || !SVOn) CancelHoming();
            if (Moving)
            {
                Ready.SetTime();
                return;
            }
            if (!Ready.IsTimeOverMs(20))
                return;
            if (IsCommand && IsHome)
            {
                if (Math.Abs(RealPos - CmdPos) > 10000)
                {
                    if (Ready.IsTimeOverMs(2000))
                        IsCommand = false;
                }
                IsCommand = false;
                CurIndex = CmdIndex;
                CurPos = CmdPos;
            }
        }

        public void StartHoming()
        {
            if (HomeRoughSpeed > 1000000)
                HomeRoughSpeed = 1000000;
            else if (HomeRoughSpeed < 100)
                HomeRoughSpeed = 100;
            if (HomeDetailSpeed > 500000)
                HomeDetailSpeed = 500000;
            else if (HomeDetailSpeed < 50)
                HomeDetailSpeed = 50;

            if (st.Run) return;
            if (!SVOn) return;

            st.NowStatus = MCStatus.INIT;

        }

        public void CancelHoming()
        {
            CurIndex = AlarmIndex;
            IsCommand = false;
            st.NowStatus = MCStatus.STOP;
            if (Moving)
                Hold(Stop.Sudden);
        }

        public void HomeRunning()
        {
            if(st.Run && !st.Error)
            {
                if (Alarm || Warn || !SVOn || !PowOn)
                    st.NowStatus = MCStatus.ERROR;
            }

            switch (st.NowStatus)
            {
                case MCStatus.INIT :
                    st.NowStatus = MCStatus.START;
                    break;
                case MCStatus.START:
                    if (HomeMode == HomeSensor.ABS)
                    {
                        SenOn();
                        st.NowStatus = MCStatus.SV_HOME_END;
                        break;
                    }
                    else if (HomeMode == HomeSensor.Z)
                    {
                        LtModOn();
                        Feed(HomingDir, HomeDetailSpeed);
                        st.NowStatus = MCStatus.SV_LOOK_Z_PHASE;
                        break;
                    }
                    if (HomingDir == Dir.CW)
                    {
                        if (HomeMode == HomeSensor.ORG || HomeMode == (HomeSensor.ORG | HomeSensor.Z))
                        {
                            if (!ORGOn)
                                Feed(HomingDir, HomeRoughSpeed);
                            st.NowStatus = MCStatus.SV_GO_ORG_SENSOR;
                        }
                        else
                        {
                            if (!CWOn)
                                Feed(HomingDir, HomeRoughSpeed);
                            st.NowStatus = MCStatus.SV_GO_CW_SENSOR;
                        }
                    }
                    else //HomingDir == Dir.CCW
                    {
                        if (HomeMode == HomeSensor.ORG || HomeMode == (HomeSensor.ORG | HomeSensor.Z))
                        {
                            if (!ORGOn)
                                Feed(HomingDir, HomeRoughSpeed);
                            st.NowStatus = MCStatus.SV_GO_ORG_SENSOR;
                        }
                        else
                        {
                            if (!CCWOn)
                                Feed(HomingDir, HomeRoughSpeed);
                            st.NowStatus = MCStatus.SV_GO_CCW_SENSOR;
                        }   
                    }
                    break;
                case MCStatus.SV_GO_ORG_SENSOR:
                    if (ORGOn)
                    {
                        Hold(Stop.Sudden);
                        st.NowStatus = MCStatus.SV_STOP_ORG_SENSOR;
                        break;
                    }
                    if ((HomingDir == Dir.CW && CWOn) || (HomingDir == Dir.CCW && CCWOn))
                    {
                        Hold(Stop.Sudden);
                        st.NowStatus = MCStatus.ERROR;
                        break;
                    }
                    break;
                case MCStatus.SV_STOP_ORG_SENSOR:
                    if(!Moving)
                        break;
                    if(!st.DelayMs(1000))
                        break;

                    Feed(HomingDir, HomeDetailSpeed, true);
                    st.NowStatus = MCStatus.SV_OUT_ORG_SENSOR;
                    break;
                case MCStatus.SV_OUT_ORG_SENSOR:
                    if (st.DelayMs(60000))
                    {
                        st.NowStatus = MCStatus.ERROR;
                        break;
                    }
                    if (!ORGOn)
                    {
                        if (HomeMode == HomeSensor.ORG)
                        {
                            Hold(Stop.Sudden);
                            st.NowStatus = MCStatus.SV_END_POS_SET;
                        }
                        else
                        {
                            LtModOn();
                            st.NowStatus = MCStatus.SV_LOOK_Z_PHASE;
                        }
                    }
                    break;
                case MCStatus.SV_GO_CW_SENSOR:
                    if (CWOn)
                    {
                        Hold(Stop.Sudden);
                        st.NowStatus = MCStatus.SV_STOP_CW_SENSOR;
                    }
                    break;
                case MCStatus.SV_STOP_CW_SENSOR:
                    if(Moving)
                        break;
                    if(!st.DelayMs(1000))
                        break;

                    Feed(HomingDir, HomeDetailSpeed, true);
                    st.NowStatus = MCStatus.SV_OUT_CW_SENSOR;
                    break;
                case MCStatus.SV_OUT_CW_SENSOR:
                    if (st.DelayMs(30000))
                    {
                        st.NowStatus = MCStatus.ERROR;
                        break;
                    }
                    if (!CWOn)
                    {
                        if (HomeMode == (HomeSensor.OT | HomeSensor.Z))
                        {
                            LtModOn();
                            st.NowStatus = MCStatus.SV_LOOK_Z_PHASE;
                        }
                        else
                        {
                            Hold(Stop.Sudden);
                            st.NowStatus = MCStatus.SV_END_POS_SET;
                        }
                    }
                    break;
                case MCStatus.SV_GO_CCW_SENSOR:
                    if (CCWOn)
                    {
                        Hold(Stop.Sudden);
                        st.NowStatus = MCStatus.SV_STOP_CCW_SENSOR;
                    }
                    break;
                case MCStatus.SV_STOP_CCW_SENSOR:
                    if(Moving)
                        break;
                    if(!st.DelayMs(1000))
                        break;
                    Feed(HomingDir, HomeDetailSpeed, true);
                    st.NowStatus = MCStatus.SV_OUT_CCW_SENSOR;
                    break;
                case MCStatus.SV_OUT_CCW_SENSOR:
                    if (st.DelayMs(30000))
                    {
                        st.NowStatus = MCStatus.ERROR;
                        break;
                    }
                    if (!CCWOn)
                    {
                        if (HomeMode == (HomeSensor.OT | HomeSensor.Z))
                        {
                            LtModOn();
                            st.NowStatus = MCStatus.SV_LOOK_Z_PHASE;
                        }
                        else
                        {
                            Hold(Stop.Sudden);
                            st.NowStatus = MCStatus.SV_END_POS_SET;
                        }
                    }
                    break;
                case MCStatus.SV_LOOK_Z_PHASE:
                    if (LCmpOn)
                    {
                        Hold(Stop.Sudden);
                        LtModOff();
                        st.NowStatus = MCStatus.SV_END_POS_SET;
                    }
                    break;
                case MCStatus.SV_END_POS_SET:
                    {
                        long lTemp = 0;
                        long lLatch = 0;
                        if(Moving)
                            break;
                        if (!st.DelayMs(2000))
                            return;
                        if (HomeMode == HomeSensor.OT || HomeMode == HomeSensor.ORG)
                        {
                            PosSet(Moni.MOT_SUBC_SET_REF | Moni.APOS, 0);
                        }
                        else
                        {
                            lLatch = GetLatchCount();
                            lTemp = RealPos - lLatch;
                            PosSet(Moni.MOT_SUBC_SET_REF | Moni.APOS, lTemp);
                        }
                        IsHome = true;
                        st.NowStatus = MCStatus.SV_HOME_END;
                    }
                    break;
                case MCStatus.SV_HOME_END:
                    if (!st.DelayMs(500))
                        return;
                    MoveIndex(DefaultIndex);
                    st.NowStatus = MCStatus.STOP;
                    break;
                case MCStatus.ERROR:
                default:
                    Hold(Stop.Sudden);
                    st.NowStatus = MCStatus.STOP;
                    break;
             
            }
        }

        public bool MoveIndex(uint index, long pos = 0, long speed = 0)
        {
            if (!IsRdy) return false;

            IsCommand = true;
            CmdIndex = index;

            if (pos == 0)
                CmdPos = PosTable[index];
            else
                CmdPos = pos;

            if (speed == 0)
                CmdSpeed = SpeedTable[index];
            else
                CmdSpeed = speed;

            if (CmdPos > RealPos)
                MoveDir = Dir.CW;
            else
                MoveDir = Dir.CCW;

            SetPosRefFilter(AccFilter.OPT_ACCFIL_SCURVE);
            SetAccDec(AccValue);
            SetMovAvrFilter(MoveAvrFilter);
            Posing(CmdPos, CmdSpeed);
            Ready.SetTime();
            return true;
        }


        public long GetRealCount()
        {
            return T1.GetRealCount(MtPort, MtNum);
        }

        public long GetLatchCount()
        {
            return T1.GetLatchCount(MtPort, MtNum);
        }

        public void SetRealCount(long pos)
        {
            T1.SetRealCount(MtPort, MtNum, pos);
        }

        public bool SenOn()
        {
            return T1.SENS_ON(MtPort, MtNum);
        }

        public bool LtModOn()
        {
            return T1.LTMOD_ON(MtPort, MtNum, (char)0);   // latch_sig_z
        }

        public bool PosSet(Moni moni, long pos)
        {
            return T1.POS_SET(MtPort, MtNum, (char)moni, pos);
        }

        public bool LtModOff()
        {
            return T1.LTMOD_OFF(MtPort, MtNum);   
        }

        public bool Feed(Dir dir,long spd, bool inverse = false)
        {
            long temp = spd;
            if (dir == Dir.CCW)
                spd *= (-1);
            if(inverse)
                spd *= (-1);

            return T1.FEED(MtPort, MtNum, spd);
        }

        public bool Hold(Stop stop)
        {
            return T1.HOLD(MtPort, MtNum, (char)stop);
        }

        public void SetPosRefFilter(AccFilter accfil)
        {
            T1.SetPosRefFilter(MtPort, MtNum, (short)accfil);
        }

        public void SetAccDec(short value)
        {
            T1.SetAccelDecel(MtPort, MtNum, value, value, 0, value, value, 0);
        }

        public void SetMovAvrFilter(short value)
        {
            T1.SetMovAvrFilter(MtPort, MtNum, value);
        }

        public bool Posing(long pos, long speed)
        {
            return T1.POSING(MtPort, MtNum, pos, speed);
        }


    }
}
